#include <string>
#include <ros/ros.h>
#include <time.h>
#include "scservo/SCServo.h"
#include "std_msgs/Float64MultiArray.h"
#include "Motor_parameter.h"
#include <algorithm>


using namespace std;

double Currentpos[IDN] = {0, 0, 0, 0, 0, 0};
static void PositionCallBack(const std_msgs::Float64MultiArray::ConstPtr& msg_sub);

int main(int argc, char **argv)
{    
    double postar[6] = {0, 0, 0, 0, 0, 0};
    double home_vel[6] = {0.02, 0.02, 0.02, 0.02, 0.02, 0.02};
    int Home_dirction[6] = {1, 1, 1, 1, 1, 1};
    double Home_time[6] = {0, 0, 0, 0, 0, 0};
    double timex = 0;
    double maxtime = 0;
    ros::init(argc, argv, "move_home");
    ros::NodeHandle node_handle;
    ros::Rate loopHZ(50);


    ros::Publisher position_home = node_handle.advertise<std_msgs::Float64MultiArray>("/fobot_joint_position_controller", 1000);
    ros::Subscriber pos_sub = node_handle.subscribe("/fobot_joint_position_publisher", 1000, PositionCallBack);
    std_msgs::Float64MultiArray init_pos;
    init_pos.data.push_back(0);
    init_pos.data.push_back(0);
    init_pos.data.push_back(0);
    init_pos.data.push_back(0);
    init_pos.data.push_back(0);
    init_pos.data.push_back(0);
    sleep(1);

    ros::spinOnce();

   

    std::cout<<Currentpos[0]<<std::endl;
    std::cout<<Currentpos[1]<<std::endl;
    std::cout<<Currentpos[2]<<std::endl;
    std::cout<<Currentpos[3]<<std::endl;
    std::cout<<Currentpos[4]<<std::endl;
    std::cout<<Currentpos[5]<<std::endl;
    



    while(timex<=(2))
    {
        
        timex +=0.01;
        for(int i=0;i<6;i++)
	    {
            init_pos.data.at(i) = Currentpos[i] + timex*timex*0.09817-0.03272*timex*timex*timex;
	    }

	    position_home.publish(init_pos);


	    loopHZ.sleep();
    }

}

void PositionCallBack(const std_msgs::Float64MultiArray::ConstPtr& msg_sub)
{
	int i = 0;
	ROS_INFO("I have known the position.");

	for (i = 0; i < 6; i++)
	{
		Currentpos[i] = msg_sub->data.at(i);
	}
	return;
}